#pragma once

#include "vector.h"
#include "matrix.h"
#include <algorithm>
#include <functional>
#include <cassert>
//for IO operations #include<glib/g3/io.h>

namespace g3 {

//symmetric matrix
template <class T>
class basic_smatrix;

typedef basic_smatrix<double> smatrix;
typedef basic_smatrix<float> smatrixf;
typedef basic_smatrix<long double> smatrixl;

template <class T>
class basic_smatrix {
public:
    T xx, xy, xz, yy, yz, zz;
public:
	typedef T * iterator;
	typedef const T * const_iterator;
public:
	T * begin()
		{ return &xx; }
	const T * begin() const
		{ return &xx; }
	T * end()
		{ return &zz + 1; }
	const T * end() const
		{ return &zz + 1; }
	static int size() 
		{ return 6; }
	static int const diagonal_elements = 3;
public:
	basic_smatrix()
		{ }
	basic_smatrix(T _xx, T _xy, T _xz, T _yy, T _yz, T _zz) :
		xx(_xx), xy(_xy), xz(_xz), yy(_yy), yz(_yz), zz(_zz)
		{ }
	template <class U> 
	basic_smatrix(const basic_smatrix<U> & v) :
		xx(T(v.xx)), xy(T(v.xy)), xz(T(v.xz)), yy(T(v.yy)), yz(T(v.yz)), zz(T(v.zz))
		{ }
	basic_smatrix & identity(T diagval = 1) 
		{ xx = yy = zz = diagval; xy = xz = yz = 0; return * this; }
	basic_smatrix & zero() 
		{ std::fill(begin(), end(), T(0)); return * this; } 
public:
	basic_vector<T> operator ()(const basic_vector<T> & v) const {
		return basic_vector<T>(xx * v.x + xy * v.y + xz * v.z,
			xy * v.x + yy * v.y + yz * v.z, xz * v.x + yz * v.y + zz * v.z);
	}
public:
	T det() const
		{ return xx*yy*zz + 2*xy*xz*yz - xx*yz*yz - yy*xz*xz - zz*xy*xy; };
	operator bool() const ///< true if not degenerate
		{ return det() != 0; };
	bool positive_defined() const
		{ return xx > 0 && xx*yy - xy*xy > 0 && det() > 0; }
	bool nonnegative_defined() const
		{ return xx >= 0 && xx*yy - xy*xy >= 0 && det() >= 0; }
	basic_smatrix<T> inverse() const;
	///this version uses eigens to provide meaningful result even for singular matrices
	basic_smatrix<T> inverse0(T eps = 0) const;
	T tr() const ///< trace of the matrix
		{ return xx + yy + zz; }
	///finds all 3 eigenvalues: min <= mid <= max; returns the count of distinct eigenvalues: 1, 2 or 3
	int eigenvalues(T * min, T * mid, T * max) const;
	///returns eigen vector for the given aliquant eugen value
	basic_vector<T> eigenvector(T eigenvalue, bool normalize = false) const;
	///finds all eigenvalues (and places them in w), and orthogonal matrix of eigenvectors in columns:
	///(*this) = u * diag(w) * u^T
	void eigens(basic_vector<T> & w, basic_matrix<T> & u) const;
public:
    basic_smatrix & operator += (const basic_smatrix<T> & v)
		{ std::transform(begin(), end(), v.begin(), begin(), std::plus<T>()); return * this; }
    basic_smatrix & operator += (T a) ///< adds identity matrix multiplied on a
		{ xx += a; yy += a; zz += a; return * this; }
    basic_smatrix & operator -= (const basic_smatrix<T> & v)
		{ std::transform(begin(), end(), v.begin(), begin(), std::minus<T>()); return * this; }
    basic_smatrix & operator -= (T a)
		{ xx -= a; yy -= a; zz -= a; return * this; }
    basic_smatrix & operator *= (T a)
		{ std::transform(begin(), end(), begin(), std::bind2nd(std::multiplies<T>(), a)); return * this; }
    basic_smatrix & operator /= (T a)
		{ std::transform(begin(), end(), begin(), std::bind2nd(std::divides<T>(), a)); return * this; }
	bool operator == (const basic_smatrix<T> & v) const
		{ return std::equal(begin(), end(), v.begin()); }
	bool operator != (const basic_smatrix<T> & v) const
		{ return !(*this == v); }
};

template <class T>
inline basic_smatrix<T> basic_smatrix<T>::inverse() const {
	return basic_smatrix<T>(
		yy*zz - yz*yz, //xx
		xz*yz - xy*zz, //xy
		xy*yz - xz*yy, //xz
		xx*zz - xz*xz, //yy
		xy*xz - xx*yz, //yz
		xx*yy - xy*xy  //zz
	) / det();
}

template <class T>
basic_smatrix<T> basic_smatrix<T>::inverse0(T eps) const {
	basic_vector<T> w;
	basic_matrix<T> u;
	eigens(w, u);
	// return u * diag(1 / w) * u^T
	if (eps == 0)
		eps = std::numeric_limits<T>::epsilon() * (w.z - w.x);
	assert (eps >= 0);
	if (fabs(w.x) <= eps) w.x = 0; else	w.x = 1 / w.x;
	if (fabs(w.y) <= eps) w.y = 0; else	w.y = 1 / w.y;
	if (fabs(w.z) <= eps) w.z = 0; else	w.z = 1 / w.z;
	return basic_smatrix<T>(
		u.e[0][0] * w.x * u.e[0][0] + u.e[0][1] * w.y * u.e[0][1] + u.e[0][2] * w.z * u.e[0][2], //xx
		u.e[0][0] * w.x * u.e[1][0] + u.e[0][1] * w.y * u.e[1][1] + u.e[0][2] * w.z * u.e[1][2], //xy
		u.e[0][0] * w.x * u.e[2][0] + u.e[0][1] * w.y * u.e[2][1] + u.e[0][2] * w.z * u.e[2][2], //xz
		u.e[1][0] * w.x * u.e[1][0] + u.e[1][1] * w.y * u.e[1][1] + u.e[1][2] * w.z * u.e[1][2], //yy
		u.e[1][0] * w.x * u.e[2][0] + u.e[1][1] * w.y * u.e[2][1] + u.e[1][2] * w.z * u.e[2][2], //yz
		u.e[2][0] * w.x * u.e[2][0] + u.e[2][1] * w.y * u.e[2][1] + u.e[2][2] * w.z * u.e[2][2]);//zz
}

template <class T>
int basic_smatrix<T>::eigenvalues(T * min, T * mid, T * max) const
{
	//canonical equation: x^3 + a x^2 + b x + c = 0
	T a = -tr();
	T third_a = a / 3;
	T b = xx * yy + yy * zz + zz * xx - xy * xy - xz * xz - yz * yz;

	T Q = (a * a - 3 * b) / 9;
	assert (Q >= 0);
	//T Q = (sqr(xx - yy) + sqr(yy - zz) + sqr(zz - xx) + 6 * (xy * xy + yz * yz + xz * xz)) / 18;
	if (Q <= std::numeric_limits<T>::epsilon() * third_a * third_a) {
		//three equal roots
		if (min) *min = -third_a;
		if (mid) *mid = -third_a;
		if (max) *max = -third_a;
		return 1;
	}
	T Q3 = Q * Q * Q;
	T c = xx * yz * yz + yy * xz * xz + zz * xy * xy - xx * yy * zz - 2 * xy * yz * xz;
	T R = (2 * a * a * a - 9 * a * b + 27 * c) / 54;
	T R2 = R * R;
	//assert (R2 <= Q3); //can be violated due to rounding errors
	T sqrtQ = sqrt(Q);
	if (R2 + 10 * std::numeric_limits<T>::epsilon() * Q3 >= Q3) {
		if (R > 0) {
			//R = Q^3/2 => phi = 0 => maximum and middle roots are equal
			if (min) *min = -2 * sqrtQ - third_a;
			if (mid) *mid = sqrtQ - third_a;
			if (max) *max = sqrtQ - third_a;
		}
		else {
			//R = -Q^3/2 => phi = pi => minimum and middle roots are equal
			if (min) *min = -sqrtQ - third_a;
			if (mid) *mid = -sqrtQ - third_a;
			if (max) *max = 2 * sqrtQ - third_a;
		}
		return 2;
	}
	//three distinct roots
	T third_phi = acos(R / (sqrtQ * sqrtQ * sqrtQ)) / 3;
	T two_third_pi = T(3.1415926535897932384626433832795 * 2 / 3);
	if (min) *min = -2 * sqrtQ * cos(third_phi) - third_a;
	if (mid) *mid = -2 * sqrtQ * cos(third_phi - two_third_pi) - third_a;
	if (max) *max = -2 * sqrtQ * cos(third_phi + two_third_pi) - third_a;
	assert (!min || !mid || *min < *mid);
	assert (!mid || !max || *mid < *max);
	return 3;
}

template <class T>
basic_vector<T> basic_smatrix<T>::eigenvector(T eigenvalue, bool normalize) const
{
	typedef basic_vector<T> vector;
	vector o1(xx - eigenvalue, xy, xz);
	vector o2(xy, yy - eigenvalue, yz);
	vector o3(xz, yz, zz - eigenvalue);
	vector p1 = outer_product(o1, o2);
	vector p2 = outer_product(o2, o3);
	vector p3 = outer_product(o3, o1);
	T pp1 = sqr(p1);
	T pp2 = sqr(p2);
	T pp3 = sqr(p3);
	if (pp1 >= pp2) {
		if (pp1 >= pp3)
			return normalize ? (1 / sqrt(pp1)) * p1 : p1;
		else
			return normalize ? (1 / sqrt(pp3)) * p3 : p3;
	}
	else {
		if (pp2 >= pp3)
			return normalize ? (1 / sqrt(pp2)) * p2 : p2;
		else
			return normalize ? (1 / sqrt(pp3)) * p3 : p3;
	}
}

template <class T>
void basic_smatrix<T>::eigens(basic_vector<T> & w, basic_matrix<T> & u) const
{
	switch (eigenvalues(&w.x, &w.y, &w.z)) {
	default:
		assert (false);
	case 3:
		assert (w.x < w.y && w.y < w.z);
		u.set_col(0, eigenvector(w.x, true));
		u.set_col(2, eigenvector(w.z, true));
		u.set_col(1, outer_product(u.col(2), u.col(0)));
		break;
	case 2:
		if (w.x == w.y) {
			assert (w.y < w.z);
			basic_vector<T> c2 = eigenvector(w.z, true);
			basic_vector<T> c1 = c2.orthogonal();
			c1 *= 1 / absolute(c1);
			u.set_col(0, outer_product(c1, c2));
			u.set_col(1, c1);
			u.set_col(2, c2);
		}
		else {
			assert (w.x < w.y && w.y == w.z);
			basic_vector<T> c0 = eigenvector(w.x, true);
			basic_vector<T> c1 = c0.orthogonal();
			c1 *= 1 / absolute(c1);
			u.set_col(0, c0);
			u.set_col(1, c1);
			u.set_col(2, outer_product(c0, c1));
		}
		break;
	case 1:
		assert (w.x == w.y && w.y == w.z);
		u.identity();
		break;
	};
}

template <class T>
inline basic_smatrix<T> operator + (const basic_smatrix<T> & a, const basic_smatrix<T> & b) {
	basic_smatrix<T> t(a);
    return t += b;
}

template <class T>
inline basic_smatrix<T> operator + (const basic_smatrix<T> & v, T a) {
	basic_smatrix<T> t(v);
    return t += a;
}

template <class T>
inline basic_smatrix<T> operator - (const basic_smatrix<T> & a, const basic_smatrix<T> & b) {
	basic_smatrix<T> t(a);
    return t -= b;
}

template <class T>
inline basic_smatrix<T> operator - (const basic_smatrix<T> & v, T a) {
	basic_smatrix<T> t(v);
    return t -= a;
}

//unary minus
template <class T>
inline basic_smatrix<T> operator - (const basic_smatrix<T> & a) {
	return basic_smatrix<T>(-a.xx, -a.xy, -a.xz, -a.yy, -a.yz, -a.zz);
}

template <class T>
inline basic_smatrix<T> operator * (const basic_smatrix<T> & v, T a) {
	basic_smatrix<T> t(v);
    return t *= a;
}

template <class T>
inline basic_smatrix<T> operator * (T a, const basic_smatrix<T> & v) {
	basic_smatrix<T> t(v);
    return t *= a;
}

template <class T>
inline basic_vector<T> operator * (const basic_smatrix<T> & m, const basic_vector<T> & v) {
    return m(v);
}

template <class T>
inline basic_smatrix<T> operator / (const basic_smatrix<T> & v, T a) {
	basic_smatrix<T> t(v);
    return t /= a;
}

///tensor square (a*a)
template <class T>
inline basic_smatrix<T> tsquare(const basic_vector<T> & a) {
	return basic_smatrix<T>(
		a.x * a.x, //xx
		a.x * a.y, //xy
		a.x * a.z, //xz
		a.y * a.y, //yy
		a.y * a.z, //yz
		a.z * a.z  //zz
	);
}

///symmetric sum of tensor products (a*b + b*a)
template <class T>
inline basic_smatrix<T> sproduct(const basic_vector<T> & a, const basic_vector<T> & b) {
	return basic_smatrix<T>(
		2 * a.x * b.x, //xx
		a.x * b.y + a.y * b.x, //xy
		a.x * b.z + a.z * b.x, //xz
		2 * a.y * b.y, //yy
		a.y * b.z + a.z * b.y, //yz
		2 * a.z * b.z  //zz
	);
}

///double convolution
template <class T>
inline T dscalar_product(const basic_smatrix<T> & v1, const basic_smatrix<T> & v2) {
	return 
		v1.xx * v2.xx + 
		v1.yy * v2.yy + 
		v1.zz * v2.zz + 
		2 * v1.xy * v2.xy +
		2 * v1.xz * v2.xz +
		2 * v1.yz * v2.yz;
}
	
/// solves system of linear equations with symmetrix matrix: a * x = b, returns x;
/// provides the solution with minimum norm of b in the case of singular a
template <class T>
basic_vector<T> sle0(const basic_smatrix<T> & a, const basic_vector<T> & b) {
	vector w;
	matrix u;
	a.eigens(w, u);
	// return u * diag(1 / w) * u^T * b
	basic_vector<T> y = b * u;
	// y := diag(1 / w) y
	T eps = std::numeric_limits<T>::epsilon() * (w.z - w.x);
	assert (eps >= 0);
	if (fabs(w.x) <= eps) y.x = 0; else	y.x /= w.x;
	if (fabs(w.y) <= eps) y.y = 0; else	y.y /= w.y;
	if (fabs(w.z) <= eps) y.z = 0; else	y.z /= w.z;
	return u * y;
}

} //namespace g3

template <class T>
class squarer<g3::basic_smatrix<T> > {
	T s;
public:
	explicit squarer(const g3::basic_smatrix<T> & v) : 
		s(v.xx*v.xx + v.yy*v.yy + v.zz*v.zz + 2*(v.xy*v.xy + v.xz*v.xz + v.yz*v.yz)) 
		{ }
	operator T () const { return s; }
};

template <class T>
class absoluter<g3::basic_smatrix<T> > {
	T a;
public:
	explicit absoluter(const g3::basic_smatrix<T> & v) : a(::sqrt(sqr(v))) { }
	operator T () const { return a; }
};
